#include "motorcontrol.h"
#include "gpio.h"
#include "types.h"
#include "system.h"

void motor_init()
{
    SET_BIT(P2MDOUT, BIT_0, 1);                //x dir
    SET_BIT(P2MDOUT, BIT_1, 1);                //x pul
    SET_BIT(P2MDOUT, BIT_2, 1);                //y dir
    SET_BIT(P2MDOUT, BIT_3, 1);                //y pul
}

void motor_shift_gear(MOTOR_INDEX_T id, MOTOR_DIRECTION_T dir, MOTOR_GEAR_T gear)
{
    switch(id)
    {
        case MOTOR_INDEX_X:
        {
            if(dir == MOTOR_DIRECTION_R){
                MOTOR_X_DIR_PORT = 0;
            }else{
                MOTOR_X_DIR_PORT = 1;
            }

            switch(gear)
            {
                case MOTOR_GEAR_LS:
                {
                    sys_timer2_stop();
                    sys_timer2_install(MOTOR_GEAR_LS_CNT);
                    sys_timer2_start();
                    break;
                }
                case MOTOR_GEAR_MS:
                {
                    sys_timer2_stop();
                    sys_timer2_install(MOTOR_GEAR_MS_CNT);
                    sys_timer2_start();
                    break;
                }
                case MOTOR_GEAR_HS:
                {
                    sys_timer2_stop();
                    sys_timer2_install(MOTOR_GEAR_HS_CNT);
                    sys_timer2_start();
                    break;
                }
                case MOTOR_GEAR_NULL:
                //TODO:disable motor here
                case MOTOR_GEAR_PARK:
                default:
                {
                    sys_timer2_stop();
                    break;
                }
            }


            break;
        }
        case MOTOR_INDEX_Y:
        {
            if(dir == MOTOR_DIRECTION_R){
                MOTOR_Y_DIR_PORT = 0;
            }else{
                MOTOR_Y_DIR_PORT = 1;
            }
            switch(gear)
            {
                case MOTOR_GEAR_LS:
                {
                    sys_timer3_stop();
                    sys_timer3_install(MOTOR_GEAR_LS_CNT);
                    sys_timer3_start();
                    break;
                }
                case MOTOR_GEAR_MS:
                {
                    sys_timer3_stop();
                    sys_timer3_install(MOTOR_GEAR_MS_CNT);
                    sys_timer3_start();
                    break;
                }
                case MOTOR_GEAR_HS:
                {
                    sys_timer3_stop();
                    sys_timer3_install(MOTOR_GEAR_HS_CNT);
                    sys_timer3_start();
                    break;
                }
                case MOTOR_GEAR_NULL:
                case MOTOR_GEAR_PARK:
                default:
                {
                    sys_timer3_stop();
                    break;
                }
            }

            break;
        }
    }
}

void motor_timer2_interrupt_handler (void) interrupt 5
{
   TF2H = 0;                           // Clear Timer2 interrupt flag
   MOTOR_X_PUL_PORT =~MOTOR_X_PUL_PORT;
}


void motor_timer3_interrupt_handler (void) interrupt 14
{
   //TF3H = 0;                           // Clear Timer2 interrupt flag
   SET_BIT(TMR3CN, BIT_7, 0);
   MOTOR_Y_PUL_PORT =~MOTOR_Y_PUL_PORT;
}

